#ifndef POISSON_SAMPLER_
#define POISSON_SAMPLER_

#include "ompl/base/ValidStateSampler.h"
#include "ompl/base/StateSampler.h"
#include "PoissonState.h"
#include "ompl/datastructures/NearestNeighborsGNAT.h"

namespace poisson
{
	const double SO3_SCALE = 1.0;

	enum E_CELL_STATUS
	{
		CS_NOSAMPLE = 0,
		CS_SAMPLE = 1,
	};

	class GridCell
	{
	public:
		GridCell()
		: baseId(-1), level(-1), sampleId(-1)
		{

		}

		GridCell(int cid, int lv, const std::vector<int>& idx)
		: baseId(cid), level(lv), indices(idx), sampleId(-1)
		{

		}

		E_CELL_STATUS GetStatus() const { return (sampleId == -1) ? CS_NOSAMPLE : CS_SAMPLE; }

		int baseId;
		int level;
		std::vector<int> indices;
		int sampleId;
	};
	
	/** \brief A state sampler that only samples valid states, uniformly. */
	class PoissonSampler : public ompl::base::ValidStateSampler
	{
	public:
		static ompl::base::ValidStateSamplerPtr allocPoissonStateSampler(const ompl::base::SpaceInformation *si);

		double distanceFunction(const ompl::base::State* a, const ompl::base::State* b) const
		{
			return si_->distance(a, b);
		}

		double distanceFunctionPoissonState(const PoissonState* a, const PoissonState* b) const
		{
			return si_->distance(a->getState(), b->getState());
		}

		/** \brief Constructor */
		PoissonSampler(const ompl::base::SpaceInformation *si);

		virtual ~PoissonSampler(void)
		{
		}

		void setup();
		void reset();

		virtual bool sample(ompl::base::State *state);
		virtual bool sampleNear(ompl::base::State *state, const ompl::base::State *near, const double distance);

		void setResetRequired(bool value) { resetRequired_ = value; }

		PoissonState* addStartPosition(const ompl::base::State* state);
		PoissonState* addGoalPosition(const ompl::base::State* state);

		int getNumStates() const { return vSamples_.size(); }

		PoissonState* getNeighborSample(PoissonState* from, PoissonState* to);
		PoissonState* collisionDetected(PoissonState* from, PoissonState* to, ompl::base::State* free);
		void stateAccepted(PoissonState* state);

		void clearSamples();

		std::vector<PoissonState*>& GetPoissonStates() { return vSamples_; }
		std::vector<PoissonState*>& GetTemplateStates() { return vvTemplateSamples_; }
		const std::vector<double>& GetBoundsMin() const { return vBoundsMin_; }
		const std::vector<double>& GetBoundsMax() const { return vBoundsMax_; }

	protected:
		ompl::base::StateSamplerPtr sampler_;

		void initializeVariables();
		void precomputeSamples();
		void getSpaceBounds();
		

		// precomputation
		void generateSamples();
		void throwDarts(std::vector<GridCell>* pCurrentActiveCells, std::vector<GridCell>* pNextActiveCells);
		void randomPointInCell(const GridCell& cell, ompl::base::State* state, double dWidth);
		bool isDiskFree(const ompl::base::State* state, const GridCell& cell, int uiSampleLevel, bool inTemplate = false) const;
		PoissonState* addState(int cid, int lv, const ompl::base::State* state, bool addToNN = true);
		void addUncoveredSubcells(GridCell& currentCell, std::vector<GridCell>* pNextActiveCells, int uiNextLevel, int uiSampleLevel) const;

		// template functions
		void initializeVariablesForTemplate();
		void createAdaptiveTemplate();


		// Utility functions
		bool isValidId(int id, int uiLevel) const;

		bool idToIndices(int id, std::vector<int>& indices, int uiLevel) const;
		bool indicesToId(const std::vector<int>& indices, int& id, int uiLevel) const;
		int positionToId(const std::vector<double>& vPos, int uiLevel) const;

		GridCell* getGridCell(int id, int uiLevel, bool create = false);
		const GridCell* getGridCell(int id, int uiLevel) const;

		// main functions
		bool subdivideDisk(PoissonState* state, ompl::base::State* free);
		void applyAdaptiveTemplate(PoissonState* state, ompl::base::State* free);

		void scaledDiff(const ompl::base::State* from, const ompl::base::State* to, ompl::base::State* diff, double scale) const;
		void scaledAdd(const ompl::base::State* from, const ompl::base::State* add, ompl::base::State* sum, double scale) const;

		bool initialized_;
		bool resetRequired_;

		ompl::RNG rng_;

		// data
		unsigned int uiDimension_;
		std::vector<double> vBoundsMin_;
		std::vector<double> vBoundsMax_;

		double dBaseRadius_;
		std::vector<double> vRadius_;
		std::vector<double> vCellWidth_;
		
		std::vector<std::vector<int> > vvDimensionSize_;
		std::vector<std::vector<int> > vvAccumulatedDimensionSize_;
		unsigned int uiNumTotalCells_;

		// precomputation
		std::vector<GridCell> vBaseCells_;
		std::vector<std::map<int, GridCell> > vvAdditionalCells_;
		std::vector<PoissonState*> vSamples_;

		// template
		std::vector<PoissonState*> vvTemplateSamples_;

		// pre-allocated memory
		std::vector<GridCell> vActiveCells_, vActiveCells2_;
		std::vector<int> vSubcellIds_;

		// planning data
		boost::shared_ptr< ompl::NearestNeighbors<PoissonState*> > nn_;
		boost::shared_ptr< ompl::NearestNeighbors<const ompl::base::State*> > nnTemplate_;

		PoissonState* startState_;
		PoissonState* goalState_;
	};
}


#endif
